/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package com.rultax.core;

import org.jbox2d.callbacks.DebugDraw;
import org.jbox2d.common.Color3f;
import org.jbox2d.common.MathUtils;
import org.jbox2d.common.OBBViewportTransform;
import org.jbox2d.common.Transform;
import org.jbox2d.common.Vec2;
import processing.core.PApplet;
import processing.core.PFont;
import processing.core.PGraphics3D;

/**
 *
 * @author scottw
 */
public class ProcessingDebugDraw extends DebugDraw{
    public static ProcessingDebugDraw screen;

    private boolean firstTime = true;
    public PApplet g;
    public PFont font;
    public float fontHeight;
    
    public ProcessingDebugDraw(PApplet pApplet){
	super(new OBBViewportTransform());
	
	g = pApplet;
	font = g.createFont("LucidaGrande-Bold", 12);
	fontHeight = 14f;
	viewportTransform.setYFlip(true);
    }   
    
    private static final Vec2 position = new Vec2();
    @Override
    public void drawPoint(Vec2 argPoint, float argRadiusOnScreen, Color3f argColor) {
	viewportTransform.getWorldToScreen(argPoint, position);
	float segments = 5f;
	float increment = 2f * MathUtils.PI / segments;
	float radius = 3f;
	float theta = 0f;
	g.fill(argColor.x, argColor.y, argColor.z);
	g.noStroke();
	g.beginShape(PApplet.POLYGON);
	for (int i = 0; i < segments; i++) {
	    float vx = position.x + radius * MathUtils.cos(theta);
	    float vy = position.y + radius * MathUtils.sin(theta);
	    g.vertex(vx, vy);
	    theta += increment;
	}
	g.endShape();
    }

    private static final Vec2 v = new Vec2();
    private static final Vec2 v1 = new Vec2();
    private static final Vec2 v2 = new Vec2();
    @Override
    public void drawSolidPolygon(Vec2[] vertices, int vertexCount, Color3f color) {
	g.noStroke();
	g.fill(0.5f * color.x, 0.5f * color.y, 0.5f * color.z, 0.5f * 255.0f);
	g.beginShape(PApplet.POLYGON);
	for (int i = 0; i < vertexCount; i++) {
	    int ind = (i + 1 < vertexCount) ? i + 1 : i + 1 - vertexCount;
	    viewportTransform.getWorldToScreen(vertices[i], v1);
	    viewportTransform.getWorldToScreen(vertices[ind], v2);
	    g.line(v1.x, v1.y, v2.x, v2.y);
	}
    }

    private static final Vec2 circlePt = new Vec2();
    @Override
    public void drawCircle(Vec2 argCenter, float radius, Color3f color) {
	viewportTransform.getWorldToScreen(argCenter, center);
	
	g.noFill();
	float segments = 16f;
	float increment = 2.0f * MathUtils.PI / segments;
	float theta = 0f;
	g.stroke(color.x, color.y, color.z);
	g.noFill();
	g.beginShape(PApplet.POLYGON);
	for (int i = 0; i < segments; i++) {
	    float vx = radius * MathUtils.cos(theta);
	    float vy = radius * MathUtils.sin(theta);
	    circlePt.set(vx, vy);
	    viewportTransform.getWorldVectorToScreen(circlePt, circlePt);
	    circlePt.addLocal(center);
	    g.vertex(circlePt.x, circlePt.y);
	    theta += increment;
	}
	circlePt.set(radius, 0);
	viewportTransform.getWorldVectorToScreen(circlePt, circlePt);
	circlePt.addLocal(center);
	g.vertex(circlePt.x, circlePt.y);
	g.endShape();
    }

    private static final Vec2 p = new Vec2();
    private static final Vec2 center = new Vec2();
    @Override
    public void drawSolidCircle(Vec2 argCenter, float radius, Vec2 axis, Color3f color) {
	viewportTransform.getWorldToScreen(argCenter, center);
	
	float segments = 16f;
	float increment = 2.0f * MathUtils.PI / segments;
	float theta = 0f;
	g.fill(0.5f * color.x, 0.5f * color.y, 0.5f * color.z, 0.5f * 255.0f);
	g.stroke(color.x, color.y, color.z, 255.0f);
	g.beginShape(PApplet.POLYGON);
	for (int i = 0; i < segments; i++) {
	    float vx = radius * MathUtils.cos(theta);
	    float vy = radius * MathUtils.sin(theta);
	    circlePt.set(vx, vy);
	    viewportTransform.getWorldVectorToScreen(circlePt, circlePt);
	    circlePt.addLocal(center);
	    g.vertex(circlePt.x, circlePt.y);
	    theta += increment;
	}
	circlePt.set(radius, 0);
	viewportTransform.getWorldVectorToScreen(circlePt, circlePt);
	circlePt.addLocal(center);
	g.vertex(circlePt.x ,circlePt.y);
	g.endShape();
	
	getWorldToScreenToOut(argCenter.x + radius * axis.x, argCenter.y + radius * axis.y, p);
	g.beginShape(PApplet.LINES);
	g.vertex(center.x, center.y);
	g.vertex(p.x, p.y);
	g.endShape();
    }

    @Override
    public void drawSegment(Vec2 argP1, Vec2 argP2, Color3f color) {
	viewportTransform.getWorldToScreen(argP1, p1);
	viewportTransform.getWorldToScreen(argP2, p2);
	g.stroke(color.x, color.y, color.z);
	g.beginShape(PApplet.LINES);
	g.vertex(p1.x, p1.y);
	g.vertex(p2.x, p2.y);
	g.endShape();
    }

    private static final Vec2 p1 = new Vec2();
    private static final Vec2 p2 = new Vec2();
    private static final Vec2 p1world = new Vec2();
    private static final Vec2 p2world = new Vec2();
    @Override
    public void drawTransform(Transform xf) {
	p1.set(xf.position);
	p2.setZero();
	float axisScale = 0.4f;
	g.beginShape(PApplet.LINES);
	viewportTransform.getWorldToScreen(p1, p1world);
	g.stroke(255.0f, 0f, 0f);
	g.vertex(p1world.x, p1world.y);
	p2.x = p1.x + axisScale * xf.R.col1.x;
	p2.y = p1.y + axisScale * xf.R.col1.y;
	viewportTransform.getWorldToScreen(p2, p2world);
	g.vertex(p2world.x, p2world.y);
	
	g.stroke(0f, 255f, 0f);
	g.vertex(p1world.x, p1world.y);
	p2.x = p1.x + axisScale * xf.R.col2.x;
	p2.y = p1.y + axisScale * xf.R.col2.y;
	viewportTransform.getWorldToScreen(p2, p2world);
	g.vertex(p2world.x, p2world.y);
	
	g.endShape();
    }

    @Override
    public void drawString(float x, float y, String s, Color3f color) {
	if(firstTime){
	    g.textFont(font);
	    if(g.g instanceof PGraphics3D) g.textMode(PApplet.SCREEN);
	    firstTime = false;
	}
	g.fill(color.x, color.y, color.z);
	g.text(s, x, y);
    }
}
